import ode
import sys
import math
from values import *
 
class Phisics:

    '''Constructor de la Classe de la Fisica'''
    def __init__(self,gamestate):
        self.init = 1
        self.itera = 0
        self.__regenerate(gamestate)

    '''Funcio per treure la posicio de totes les posicions dels elements'''
    def __logSystem(self):
        print "BALL ::::::::---------------------------------"
        print self.ball.getPosition()
        print "PLAYERS :::::::---------------------------------"
        for team in range(2):
            for i in self.players[team]:
                pos= self.players[team][i].getPosition()
                print " - %s7 --> X (%f)    Y (%f)   Z(%f)"%(i,pos[0],pos[1],pos[2])
                rot= self.players[team][i].getRotation()

        #print self.floor.getPosition()
        print "WALLS :::::::---------------------------------"
        for i in self.walls:
            pos= self.walls[i].getPosition()
            print " - %s7  --> X (%f)   Y (%f)   Z(%f)  "%(i,pos[0],pos[1],pos[2])
   

    '''Reposicionament dels jugadors i de la pilota, quan hi hagi falta o Gol'''
    def __reposition(self,gamestate):
        posBall=self.__transformPos(gamestate.ball.position)
        self.ball.setPosition((posBall[0],posBall[1],RADIUS+0.001))
        self.ball.setLinearVel(INI_VEL)
        for player in gamestate.teamA:
            pos =self.__transformPos(gamestate.teamA[player].position)
            self.players[0][player].setPosition((pos[0],pos[1],DIMEN[2]+0.0001))
            self.players[0][player].setLinearVel(INI_VEL_P)

        for player in gamestate.teamB:
            pos =self.__transformPos(gamestate.teamB[player].position)
            self.players[1][player].setPosition((pos[0],pos[1],DIMEN[2]+0.0001))
            self.players[1][player].setLinearVel(INI_VEL_P)



    '''Computacio d'un nou estat'''
    def compute(self, gamestate, time, relocate=0):
        if relocate != 0:
            print " ========================== REPOSICIONANT ================================"
            self.__reposition(gamestate)
            self.__logSystem()
            print " ========================================================================="
        else:
            self.__applyValues(gamestate)
            self.space.collide(None, self.__near_callback)
            self.__logSystem()
            #self.__applyJoinPlane()
            self.world.step(time)
            self.contactgroup.empty()
            return self.__saveState(gamestate)
        return


    '''Canvi dels valors que es dessitgen sobre els jugadors'''
    def __applyValues(self,gamestate):
        teamA=gamestate.teamA
        teamB=gamestate.teamB
        for player in gamestate.teamA:
            vel =self.__trans_velAng(gamestate.teamA[player].linearVel,gamestate.teamA[player].direction)
            if abs(vel[0]) > MAX_VEL :
                vel_x = MAX_VEL*(vel[0]/abs(vel[0]))
            else:
                vel_x = vel[0]
            if abs(vel[1]) > MAX_VEL :
                vel_y = MAX_VEL*(vel[1]/abs(vel[0]))
            else:
                vel_y = vel[1]
            self.players[0][player].setLinearVel((vel_x,vel_y,0.))
            ang_vel = (math.pi*gamestate.teamA[player].angVel)/180.
            self.players[0][player].setAngularVel((0.,0.,ang_vel))
        for player in gamestate.teamB:
            vel =self.__trans_velAng(gamestate.teamB[player].linearVel,gamestate.teamB[player].direction)
            if abs(vel[0]) > MAX_VEL :
                vel_x = MAX_VEL*(vel[0]/abs(vel[0]))
            else:
                vel_x=vel[0]
            if abs(vel[1]) > MAX_VEL :
                vel_y = MAX_VEL*(vel[1]/abs(vel[0]))
            else:
                vel_x=vel[1]
            self.players[1][player].setLinearVel((vel_x,vel_y,0.))
            ang_vel = (math.pi*gamestate.teamB[player].angVel)/180.
            self.players[1][player].setAngularVel((0.,0.,ang_vel))
    
    '''Recuperacio de l'escala original del valor de posicio de la pilota'''
    def __restoreUnitBall(self,x,y):
        return (x*1000,y*1000)


    '''Retornem l'estat salvat de la pilota'''
    def __saveState(self,gamestate):
        pos=self.ball.getPosition()
        gamestate.ball.position=self.__restoreUnitBall(pos[0],pos[1])
        vel_ang=self.__getLinearVel(self.ball.getLinearVel(),gamestate.ball.direction)
        gamestate.ball.linearVel=vel_ang[0]
        gamestate.ball.direction=vel_ang[1]
        for team in range(2):
            for i in self.players[team]:
                pos=self.__retransformPos(self.players[team][i].getPosition())
                if team == 0: 
                    #rot= self.players[team][i].getRotation()
                    quat= self.players[team][i].getQuaternion()
                    gamestate.teamA[i].position=pos
                    w= quat[0]
                    angle = 2 * math.acos(w)
                    if (quat[3] > 0 and angle < 0 ) or (quat[3] < 0 and angle > 0):
                        angle -= math.pi
                    
                    '''s = math.sqrt(1-w*w)
                    if (abs(s)< 0.001): s=1
                    x = quat[1]/s
                    y = quat[2]/s
                    z = quat[3]/s'''
                    gamestate.teamA[i].direction=(angle*180)/math.pi
                    #gamestate.teamA[i].direction=((math.acos(rot[0])*180)/math.pi)
                else: 
                    quat= self.players[team][i].getQuaternion()
                    w= quat[0]
                    angle = 2 * math.acos(w)
                    if (quat[3] > 0 and angle < 0 ) or (quat[3] < 0 and angle > 0):
                        angle -= math.pi
 
                    rot= self.players[team][i].getRotation()
                    gamestate.teamB[i].position=pos
                    gamestate.teamB[i].direction=((math.acos(rot[0])*180)/math.pi)
        
        return gamestate
        
    def __applyJoinPlane(self):
        for team in range(0,1):
            for i in self.players[team]:
                rot = self.players[team][i].getAngularVel()
                quat_ptr = self.players[team][i].getQuaternion()
                quat = [0,0,0,0]
                quat[0]= quat_ptr[0]
                quat[3]= quat_ptr[3]
                quat_len = math.sqrt( quat[0]*quat[0] + quat[3] * quat[3])
                quat[0]/= quat_len
                quat[3]/= quat_len
                self.players[team][i].setQuaternion(quat)
                self.players[team][i].setAngularVel((0,0,rot[2]))


    def __near_callback(self, args, geom1, geom2):
        contacts = ode.collide(geom1, geom2)
        for c in contacts:
            c.setBounce(BOUNCE)
            c.setMu(MU)
            j = ode.ContactJoint(self.world, self.contactgroup, c)
            j.attach(geom1.getBody(), geom2.getBody())
            #self.__applyJoinPlane()
            
        vel=self.ball.getLinearVel()
        self.ball.setLinearVel(self.__fregament_pilota(vel))

        
    '''Calcul de la velocitat Lineal, de 2D a Modul'''
    def __getLinearVel(self,velLineal,direction):
        x_o=int(velLineal[0]*1000)
        y_o=int(velLineal[1]*1000)
        
        x=math.pow(x_o,2)
        y=math.pow(y_o,2)
        hipo= float(math.sqrt(x+y))
        print "X -> %f   Y -> %f   Hipo -> %f"%(x,y,hipo)
        if abs(hipo) > 0.00001:
            ang = math.atan(y_o/x_o)
            if x_o < 0:
                ang -= math.pi
            ang = (ang * 180.)/math.pi
            return (hipo,ang)
        return (0,direction)


    def __trans_velAng(self,val,ang):
        ang = (ang*math.pi)/180.
        return ((val/1000.)*math.cos(ang),(val/1000.)*math.sin(ang),ang)			


    def __fregament_pilota(self,val):
		return (val[0]-(MU_P*val[0]),val[1]-(MU_P*val[1]),0.0)


    def __regenerate(self,gamestate):
        self.num_local_player=0
        self.num_inv_player=0
        self.walls={}
        self.players={}
        # Create world
        self.__createWorld()

        # Create Walls
        for i in WALLS:
                self.__createWall(i[0],i[1],i[2])


        print gamestate
        posBall=self.__transformPos(gamestate.ball.position)

        self.__createBall((posBall[0],posBall[1],(RADIUS/2)+0.001),INI_VEL)

        self.__createPlayer(0,gamestate.teamA)
        self.__createPlayer(1,gamestate.teamB)

        self.top = ode.GeomPlane(self.space, (0,0,1), -0.06001)

    def __transformPos(self,x):
        return (x[0]/1000.,x[1]/1000.)

    def __retransformPos(self,x):
        return (int(x[0]*1000),int(x[1]*1000))

    '''PRIVATE: Create a BALL in center of CAMP'''
    def __createBall(self,pos,vel):
        self.ball = ode.Body(self.world)
        M = ode.Mass()
        self.ball.radius=RADIUS
        M.setSphere(DENSITY, RADIUS)
        self.ball.setMass(M)
        self.ball.setPosition(pos)
        geom = ode.GeomSphere(self.space, radius=RADIUS)
        self.ball._id=geom._id
        geom.setBody(self.ball)
        self.ball.setLinearVel(vel)
              

    '''PRIVATE: Create the World and the Floor'''
    def __createWorld(self):
        self.world = ode.World()
        self.world.setGravity((0, 0, -9.81))
        #self.world.setGravity((0, 0, 0))
        self.world.setERP(ERP)   # error reduction parameter
        self.world.setCFM(CFM)  # constraint force mixing
        self.space = ode.Space()
        self.floor = ode.GeomPlane(self.space, (0,0,1), 0)
        self.contactgroup = ode.JointGroup()


    '''PRIVATE: Create a Wall passed by Argument'''
    def __createWall(self,name,size,position):
        self.walls[name] = ode.Body(self.world)
        M = ode.Mass()
        M.setBox(WEIGHT_WALL, size[0],size[1],size[2])
        self.walls[name].setMass(M)
        self.walls[name].si2_position=position
        self.walls[name].setPosition(self.walls[name].si2_position)
        self.walls[name].lengths = size
        geom = ode.GeomBox(self.space, lengths=self.walls[name].lengths)
        geom.setBody(self.walls[name])
        self.walls[name].unions = ode.FixedJoint(self.world)
        self.walls[name].unions.attach(self.walls[name], ode.environment)
        self.walls[name].unions.setFixed()

    def __createPlayer(self,team,players):
        self.players[team]= {} 
        for player in players:
            self.players[team][player] = ode.Body(self.world)
            M = ode.Mass()
            M.setBox(WEIGHT_PLAYER, DIMEN[0], DIMEN[1], DIMEN[2])
            self.players[team][player].setMass(M)
            posPlayer = self.__transformPos(players[player].position)
            self.players[team][player].setPosition((posPlayer[0],posPlayer[1],(DIMEN[0]/2)+0.001))
            #self.players[team][player].union = ode.Plane2DJoint(self.world)
            #self.players[team][player].union.attack(self.players[team][player],ode.environment)
            geom = ode.GeomBox(self.space, lengths=DIMEN)
            geom.setBody(self.players[team][player])


